﻿Imports System.Text
Imports System.ComponentModel
Imports System.Runtime.InteropServices
Imports System.Threading

'2020-09-25 考虑把读和解析数据专门放一个线程

Public Class WingPort : Inherits Transport

    Private Const MutexWaitTimeout = 2000
    Private print_bytes As Action(Of Byte()) = Sub(b)
                                                   For i As Integer = 0 To b.Length - 1
                                                       Console.Write(b(i).ToString("X2") + " ")
                                                   Next
                                                   Console.Write(vbCrLf)
                                               End Sub

#Region "protocol"
    Public Shared FrameGetVersion() As Byte = New Byte(4) {&H5A, 0, &H0, &H5A, &H7E}
    Public Shared FrameResetWheels() As Byte = New Byte(4) {&H5A, 12, &H0, &H66, &H7E}
    Public Shared FrameGetRobotState() As Byte = New Byte(4) {&H5A, 11, &H0, &H65, &H7E}
    Public Shared FrameGetOdom() As Byte = New Byte(4) {&H5A, 5, &H0, &H5F, &H7E}
    Public Shared FrameClearOdom() As Byte = New Byte(4) {&H5A, 3, &H0, &H5D, &H7E}

    Public Const ID_SET_VELOCITY As Byte = &H4
    Public Const ID_GET_ODOM As Byte = &H5
    Public Const ID_SET_EACH_MOTOR As Byte = &H8
    Public Const ID_SET_ARM As Byte = &H9
    Public Const ID_SET_RAISE As Byte = 10
    Public Const ID_GET_ROBOT_STATE As Byte = 11
    Public Const ID_SET_SPIN As Byte = 13
    Public Const ID_SET_DOUBLE_CAR As Byte = 14
    Private ReadOnly TeamPosition As Dictionary(Of String, Integer) = New Dictionary(Of String, Integer) From {
        {"Front", 1},
        {"Back", 2}
    }
#End Region

    Public EachMotor As SetEachMotor_T
    Public Arm As SetHydroArm_T
    Public CarVel As SetVelocity_T
    Public Raise As SetHydroRaise_T
    Public Spin As SetSpin_T
    Public RobotState As RobotState_T
    Public Odom As Odom_T
    Public TeamSet As SetDoubleCar_T

    <Description("模拟的通信灯信号")>
    Public Property Led As Boolean

    <Description("硬件版本")>
    Public Property Version As String

    Private RecvThread As Thread
    Private recv_state As RecvState

    Private serial_lock As New Object

    Enum RecvState
        WaitHead
        WaitID
        WaitLen
        WaitData
    End Enum

    Public Sub New()
        ReDim RobotState.angles(3)
        ReDim RobotState.height(3)
    End Sub

    Public Sub BeginRead()
        RecvThread = New Thread(AddressOf RecvTask)
        RecvThread.Start()
    End Sub

    ''' <summary>
    ''' 下发电机控制指令到下位机
    ''' </summary>
    Public Sub SetEachMotor()
        SyncLock serial_lock
            SendMsg(GenerateMsg(MsgType.SetEachMotor, EachMotor))
        End SyncLock
    End Sub

    Public Sub RequestRobotState()

        Dim robot_state_len = Marshal.SizeOf(RobotState) + 5
        Dim robot_odom_len = Marshal.SizeOf(Odom) + 5

        Dim read = Sub(rd_len As Integer)
                       Dim byte_v(rd_len - 1) As Byte
                       port.Read(byte_v, 0, rd_len)
                       'print_bytes(byte_v)
                       Parse(byte_v)
                   End Sub

        Dim wait_ack = Sub(rd_len As Integer, msg As string)
                           Dim cnt = 0
                           'Console.WriteLine($"Wait {msg}")
                           While port.BytesToRead < rd_len
                               Threading.Thread.Sleep(10)
                               cnt += 1
                               If cnt > 100 Then
                                   Console.WriteLine("[ERROR] Read time out")
                                   port.DiscardInBuffer()
                                   Exit While
                               End If
                           End While
                           read(rd_len)
                       End Sub

        Dim robot_state_task = Task.Run(Sub()
                                            Console.WriteLine("[Task] request state start.")
                                            While Ready
                                                Thread.Sleep(5)
                                                Try
                                                    SyncLock serial_lock
                                                        SendMsg(FrameGetRobotState)
                                                        Thread.Sleep(5)
                                                        SendMsg(FrameGetOdom)
                                                        Led = Not Led
                                                    End SyncLock
                                                Catch ex As Exception
                                                    Console.WriteLine($"[Error] {ex.Message}")
                                                End Try
                                            End While
                                            Console.WriteLine("[Task] Exit Task.")
                                        End Sub)

    End Sub

    ''' <summary>
    ''' 控制整车速度
    ''' </summary>
    ''' <param name="vx">前进方向速度</param>
    ''' <param name="vy">右方速度</param>
    ''' <param name="rz">角速度</param>
    Public Sub SetVelocity(vx As Short, vy As Short, rz As Short)
        SyncLock serial_lock
            Me.CarVel.v_linear_x = vx
            Me.CarVel.v_linear_y = vy
            Me.CarVel.v_angular_z = rz
            SendMsg(GenerateMsg(MsgType.SetVelocity, CarVel))
        End SyncLock
    End Sub

    Public Sub SetSpin(v As Short)
        SyncLock serial_lock
            Me.Spin.wheel_vel = v
            SendMsg(GenerateMsg(MsgType.SetSpin, Spin))
        End SyncLock
    End Sub

    ''' <summary>
    ''' 控制左右抱臂状态
    ''' </summary>
    Public Sub SetArms()
        SyncLock serial_lock
            SendMsg(GenerateMsg(MsgType.SetHydroArm, Arm))
        End SyncLock
    End Sub

    ''' <summary>
    ''' 控制举升
    ''' </summary>
    Public Sub SetRaise()
        SyncLock serial_lock
            SendMsg(GenerateMsg(MsgType.SetHydroRaise, Raise))
        End SyncLock
    End Sub

    ''' <summary>
    ''' 尝试一下异步方法，其实这里没必要，因为ReadTo似乎是阻塞的
    ''' </summary>
    ''' <param name="finish"></param>
    ''' <returns></returns>
    Public Function ReadUntil(finish As String) As String
        Dim ret As String = ""
        Console.WriteLine("wait ack")
        Try
            ret = port.ReadTo(finish)
            print_bytes(Encoding.ASCII.GetBytes(ret))
            Console.WriteLine($"get ack, byte remain: {port.BytesToRead}")
        Catch ex As Exception
            Console.WriteLine($"[Error] {ex.Message}")
            port.DiscardInBuffer()
        End Try
        'Console.WriteLine(ret)
        Return ret
    End Function

    ''' <summary>
    ''' 请求硬件版本
    ''' </summary>
    Public Sub RequestVersion()
        '发送请求帧
        SyncLock serial_lock
            port.Write(FrameGetVersion, 0, FrameGetVersion.Length)
        End SyncLock
    End Sub

    Public Function ResetWheels() As Boolean
        SyncLock serial_lock
            port.Write(FrameResetWheels, 0, FrameResetWheels.Length)
        End SyncLock
        Return True
    End Function

    Public Sub ClearOdom()
        If Ready Then
            SyncLock serial_lock
                port.Write(FrameClearOdom, 0, FrameClearOdom.Length)
            End SyncLock
        End If
    End Sub

    Public Sub SetDoubleCar(enable As Boolean, position As String, distance As UInteger)
        SyncLock serial_lock
            TeamSet.enable = IIf(enable, 1, 0)
            TeamSet.position = TeamPosition(position)
            TeamSet.distance = distance
            SendMsg(GenerateMsg(MsgType.SetDoubleCar, TeamSet))
        End SyncLock
    End Sub

    ''' <summary>
    ''' 解析协议
    ''' </summary>
    ''' <param name="frame"></param>
    ''' <returns>失败返回false</returns>
    Private Function Parse(frame As Byte()) As Boolean
        Dim ret As Boolean = True

        Dim parse_to_object = Function(st As Object) As Object
                                  Dim data_len = Marshal.SizeOf(st)
                                  Dim data_bytes(data_len) As Byte
                                  Array.Copy(frame, 3, data_bytes, 0, data_len)
                                  Return Transport.ByteToStruct(data_bytes, st.GetType())
                              End Function

        If frame.Length = 0 Then
            Return False
        End If

        If frame(0) <> &H5A Then
            ret = False
        Else
            Dim msg_id As Byte = frame(1)
            Dim len As Integer = frame(2)
            Select Case msg_id
                Case &H0
                    Dim _version(15) As Byte
                    Dim _date(15) As Byte
                    Array.Copy(frame, 3, _version, 0, _version.Length)
                    Array.Copy(frame, 3 + _version.Length, _date, 0, _date.Length)
                    Me.Version = Encoding.ASCII.GetString(_version).Replace(vbNullChar, "") + "-" + Encoding.ASCII.GetString(_date).Replace(vbNullChar, "")
                    Console.WriteLine($"version: {Version}")
                Case ID_SET_EACH_MOTOR
                    Console.WriteLine("motor control success.")
                Case ID_GET_ROBOT_STATE
                    RobotState = parse_to_object(RobotState)
                Case ID_GET_ODOM
                    Odom = parse_to_object(Odom)
                    'Console.WriteLine($"vx:{Odom.v_liner_x / 100}, vth:{Odom.v_angular_z / 100 / 3.14 * 180}")
                Case Else
            End Select
        End If
        Return ret
    End Function

    ''' <summary>
    ''' 生成要发送的协议
    ''' </summary>
    ''' <param name="msg_id">信息类型</param>
    ''' <param name="data_struct">数据结构体</param>
    ''' <returns></returns>
    Public Function GenerateMsg(msg_id As MsgType, data_struct As Object) As Byte()
        Dim tx_buffer(4) As Byte
        Dim data_buf() As Byte = Nothing
        Dim data_len As Integer = 0
        tx_buffer(0) = &H5A

        data_len = Marshal.SizeOf(data_struct)
        data_buf = Transport.StructToByte(data_struct)
        tx_buffer(2) = data_len
        ReDim Preserve tx_buffer(data_len + 4)

        If msg_id = MsgType.SetHydroArm Then
            tx_buffer(1) = ID_SET_ARM
        ElseIf msg_id = MsgType.SetEachMotor Then
            tx_buffer(1) = ID_SET_EACH_MOTOR
        ElseIf msg_id = MsgType.SetVelocity Then
            tx_buffer(1) = ID_SET_VELOCITY
        ElseIf msg_id = MsgType.SetHydroRaise Then
            tx_buffer(1) = ID_SET_RAISE
        ElseIf msg_id = MsgType.SetSpin Then
            tx_buffer(1) = ID_SET_SPIN
        ElseIf msg_id = MsgType.GetRobotState Then
            tx_buffer(1) = ID_GET_ROBOT_STATE
        ElseIf msg_id = MsgType.SetDoubleCar Then
            tx_buffer(1) = ID_SET_DOUBLE_CAR
        End If

        If data_buf IsNot Nothing Then
            Array.Copy(data_buf, 0, tx_buffer, 3, data_buf.Length)
        End If
        '填入固定结束符
        Dim crc As Byte = Calculate.Sum(tx_buffer)
        tx_buffer(tx_buffer.Length - 2) = crc
        tx_buffer(tx_buffer.Length - 1) = &H7E
        Return tx_buffer
    End Function

    ''' <summary>
    ''' 下发协议
    ''' </summary>
    ''' <param name="buf">协议内容</param>
    ''' <returns></returns>
    Public Function SendMsg(buf() As Byte) As Boolean
        Dim ret As Boolean = True
        If buf.Length < 5 Then
            ret = False
        Else
            If buf(0) <> &H5A Then
                ret = False
            Else
                Dim total_len As Integer = buf(2) + 5
                Try
                    port.Write(buf, 0, total_len)
                Catch ex As Exception
                    Console.WriteLine(ex.Message)
                End Try
            End If
        End If
        Return ret
    End Function

    Private Sub RecvTask()
        recv_state = RecvState.WaitHead
        Dim head(2) As Byte
        Dim frame() As Byte
        Dim frame_len As UInteger

        While Ready
            'Thread.Sleep(1) 睡眠会导致接收不及时，影响实时性
            Try
                If port.BytesToRead > 0 Then
                    Select Case recv_state
                        Case RecvState.WaitHead
                            head(0) = port.ReadByte()
                            If head(0) = &H5A Then recv_state = RecvState.WaitID
                        Case RecvState.WaitID
                            head(1) = port.ReadByte()
                            recv_state = RecvState.WaitLen
                        Case RecvState.WaitLen
                            head(2) = port.ReadByte()
                            frame_len = head(2) + 2
                            recv_state = RecvState.WaitData
                        Case RecvState.WaitData
                            If port.BytesToRead >= frame_len Then
                                recv_state = RecvState.WaitHead
                                ReDim frame(frame_len + 2)
                                port.Read(frame, 3, frame_len)
                                Array.Copy(head, frame, 3)
                                print_bytes(frame)
                                Parse(frame)
                            End If
                    End Select
                End If
            Catch ex As Exception

            End Try
        End While
    End Sub

End Class

Public Class Calculate
    Shared Function Sum(ByVal buf() As Byte) As Byte
        Dim ret As Byte = 0
        Dim _sum As Integer = 0
        For Each b As Byte In buf
            _sum = (_sum + b) Mod 256
        Next
        ret = _sum
        Return ret
    End Function
End Class

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetHydroArm_T

    Public Structure Couple
        Public Structure Arm
            Public Strech As Byte
            Public DrawBack As Byte
            Public Pump As Byte
        End Structure

        Public ArmL As Arm
        Public ArmR As Arm
    End Structure

    Public Couples As Couple

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetEachMotor_T

    Public Structure Wheel
        Public VL As Short
        Public VR As Short
    End Structure

    Public WheelFL As Wheel
    Public WheelFR As Wheel
    Public WheelBL As Wheel
    Public WheelBR As Wheel

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetVelocity_T

    Public v_linear_x As Short
    Public v_linear_y As Short
    Public v_angular_z As Short

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetHydroRaise_T

    Public up As Byte
    Public down As Byte
    Public pump As Byte

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetSpin_T

    Public angular_vel As Short
    Public wheel_vel As Short

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure RobotState_T

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public angles() As Single

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public height() As Single

    Public small_gear As UShort
    Public big_gear As UShort
    Public team_info As SetDoubleCar_T

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetDoubleCar_T

    Public enable As Byte
    Public position As Byte     '相对位置1-前车，2-后车
    Public distance As UInteger '两车中心距(mm)

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure Odom_T

    Public v_liner_x As Short      '线速度 前>0 后<0  cm/s
    Public v_liner_y As Short      '差分轮 为0        cm/s
    Public v_angular_z As Short    '角速度 左>0 右<0  0.01rad/s   100 means 1 rad/s
    Public x As Integer            '里程计坐标x       cm (这里long为4字节， 下同)
    Public y As Integer            '里程计坐标y       cm
    Public yaw As Short            '里程计航角       0.1度

End Structure

Public Enum MsgType
    SetHydroArm
    SetHydroRaise
    SetEachMotor
    SetVelocity
    SetSpin
    GetRobotState
    SetDoubleCar
End Enum
